/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotTemplate extends SimpleRobot {
    /**
     * This function is called once each time the robot enters autonomous mode.
     */
     private RobotDrive myRobot = new RobotDrive(1,2,3,4);
     private Joystick leftDriveJoy = new Joystick(1);
     private Joystick rightDriveJoy = new Joystick(2);



    public void autonomous() {
        //I think that autonomous goes for 90 secs.
        //Nice work!
        myRobot.drive(.1, 0); //Drive forward at .1 speed
        Timer.delay(2);       //Pause for 2 seconds
        myRobot.drive(-.1, 0); // Drive backwards at .1 speed
        

    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while(isOperatorControl() && isEnabled()){
            myRobot.mecanumDrive_Cartesian(leftDriveJoy.getX(), leftDriveJoy.getY(), rightDriveJoy.getX(), 0.0);
            Timer.delay(0.005);
    }
}
}
